%FocoUS2.m

clear all
close all
clc


fo = 7.5; % Frecuencia central del Array
NbPeriodos = 1; %Numero de periodos de la emision US
Amplitud = 0.99;
Ne = 32; %Numero de elementos del Array
deads = []; %Canales muertos del Array
Nboards = Ne/8;
address = 0;
DistInic = 10 % Distancia inicial en mm
DistFin =  60 % Distancia final en mm
NbTiros = 10; %Numero de tiros US
NbSec = 1; %Numero de secuencias 
fr = 2; %Frecuencia de recurrencia en KHz
Cp = 1.5; %Velocidad de sonido en el medio en mm/us
rsf = 80; %Frecuencia de muestreo a la recepci�n en MHz
esf = 80; %Frecuencia de muestreo a la emision en MHz
bsf = 80;
FR = log2(bsf/rsf);
FE = log2(bsf/esf);
AmpEmision = 200; %Amplitud de la se�al de emision en V
Gain = 55; %Ganancia en dB
D = 11; %Apertura del array en mm
pitch = D/(Ne-1); %Pitch del array en mm


%PARA ONDA PLANA PONER FOCO LEJANO (e.g.: 10000)
Foco = 100000 
R = sqrt((D/2)^2+Foco^2); %distancia maxima entre el array y el foco
Nt = floor(NbPeriodos*esf/fo);
t = (0:1:Nt-1)/esf;
so = Amplitud*sin(2*pi*fo*t);

%PARA PARED IMPULSIVA
%so=Amplitud;

for i=1:Ne
    d = sqrt((D/2-(i-1)*pitch)^2+Foco^2);
    delay = (1/Cp)*(R-d);%delay en emision expresado en us
    delayN(i) = round(delay*esf); 
end
clear i
NDelMax = max(delayN);
for i=1:Ne
    wave(:,i) = [zeros(delayN(i),1);so';zeros(NDelMax-delayN(i),1)];
end
clear i
wave(:,deads) = 0;

%PARA IMPULSO EN UN TRANSDUCTOR
%wave(:,1:15)=0;
%wave(:,17:Ne)=0;

%''''''''''''''''''''''ATENCION''''''''''''''''''''''''''''''''''''
%LecoeurUS('Init',0,Ne,NbSec); %Usar solo en la primera ejecucion despues
%de prendido el equipo

LecoeurUS('Init_USB'); %Usar el resto de la veces
%'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
pause(0.5)
LecoeurUS('LoadEmSeq',0,Ne,wave);
LecoeurUS('SetNumFreq',FR,FE);
LecoeurUS('SoftTrigger',1);
for i=1:Ne
    LecoeurUS('Gain',Gain,i);
    LecoeurUS('Voltage',AmpEmision,i);
end
clear i

NbSamples = floor(2*rsf*(DistFin-DistInic)/Cp);
Delay = floor(2*rsf*DistInic/Cp);
espera = NbTiros*NbSec/(fr*1000); %tiempo total de ejecuci�n en s

%PGM Card Sequencer
X = 49152 + NbTiros;
seq = X*ones(1,NbSec);
seq(NbSec+1)=0;
seqDur = (NbSamples + Delay + Nt + 1000*bsf/fr)*ones(1,NbSec);
seqWidth = NbSamples*ones(1,NbSec);
seqDelay = Delay*ones(1,NbSec);
seqPdelay = NbTiros*ones(1,NbSec);
seqPseq = 0*ones(1,NbSec);

%---------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



LecoeurUS('Upload_Seq_Program',seq)

for i=1:Nboards
    LecoeurUS('Upload_Seq_Num_Width',i,seqWidth);
    LecoeurUS('Upload_Seq_Num_Delay',i,seqDelay);
    LecoeurUS('Upload_Seq_Pulser_Delay',i,seqPdelay);
    LecoeurUS('Upload_Seq_Pulser_Seq',i,seqPseq);
end
clear i
LecoeurUs('Upload_Seq_Duration',seqDur);
pause(0.1)

EjeZ = DistInic + Cp*[0:NbSamples-1]/(2*rsf);
EjeX = 0:pitch:(Ne-1)*pitch;

for i=1:3
    RF=zeros(1,NbSamples*NbTiros*NbSec);
    LecoeurUS('Exe_Sequencer',address);
    pause(espera*1.1);
    for j=1:Ne
        RF = LecoeurUS('Read_Memory',NbSamples*NbTiros*NbSec,j,address,0)-2048;
        %size(RF)
        RF2(j,:,:)=reshape(RF,NbSamples,NbSec*NbTiros);
        %clear RF
    end
    RF3=mean(RF2,3);
    RF3=RF3';
    clear j
    Sal = BeamFormingBruto(abs(hilbert(RF3)),pitch,rsf,DistInic);
    figure(1)
    Img = Sal/max(max(Sal));
    Img = 20*log10(Img);
    RatioZX = (EjeZ(end)-EjeZ(1))/(EjeX(end)-EjeX(1));
    set(1,'units','centimeters','position',[30 1 4 RatioZX*4])
    imagesc(EjeX,EjeZ,Img), caxis([-12 0]), title([num2str(i)]), drawnow, pause(0.01)
end
%----------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%